#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/exceptions.hpp"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"
#include <memory>
#include <rclcpp/rclcpp.hpp>

// 目标的坐标系变换监听器
class ListenerPoint : public rclcpp::Node {
public:
  ListenerPoint() : Node("listener_point") {
    RCLCPP_INFO(this->get_logger(), "监听器节点已启动");
    // 创建TF2的Buffer对象，融合多个坐标系相对关系为一颗坐标树
    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());

    // 创建TransformListener对象，自动订阅TF2话题并将数据存储到Buffer中
    tf_listener_ =
        std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this);

    // 创建一个定时器，每隔1秒调用一次timer_callback函数
    timer_ = this->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&ListenerPoint::timer_callback, this));
  }

private:
  void timer_callback() {

    // 定义一个TransformStamped对象，用于存储查询到的变换信息
    geometry_msgs::msg::TransformStamped transform_stamped;

    // 使用try-catch块来处理可能的异常
    try {
      // 尝试从Buffer中获取从"base_link"坐标系到"turtle1"坐标系的变换
      transform_stamped =
          tf_buffer_->lookupTransform("base_link", "point_frame", tf2::TimePointZero);

      // 如果获取成功，打印变换的平移部分
      RCLCPP_INFO(this->get_logger(),
                  "Got transform: x: %.2f, y: %.2f, z: %.2f",
                  transform_stamped.transform.translation.x,
                  transform_stamped.transform.translation.y,
                  transform_stamped.transform.translation.z);
    } catch (tf2::TransformException &ex) {
      // 如果获取变换失败，打印错误信息
      RCLCPP_WARN(this->get_logger(), "无法进行变换: %s", ex.what());
    }
  }

  // TF2 Buffer对象
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  // TransformListener对象
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  // 定时器
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ListenerPoint>());
  rclcpp::shutdown();
  return 0;
}
